//**************Compass*********************************************
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Compass.h> // Compass Library
#include <AP_Math.h>	// ArduPilot Mega Vector/Matrix math Library
#include <Wire.h>

#define ToDeg(x) (x*57.2957795131)  // *180/pi
#define ToRad(x) (x*0.01745329252)  // *pi/180

FastSerialPort0(Serial);

AP_Compass_HMC5843 compass;
//******************************************************************

long timer;
int wayDegree;

//Constants
#define JET_ZERO 1500

#define AUTOPILOT_SPEED 1550
#define AUTOPILOT_BACKLASH 10
#define AUTOPILOT_STOP_OFFSET 30


#define AUTOPILOT_TURN_HIGH_SPEED_LEFT 1650
#define AUTOPILOT_TURN_HIGH_SPEED_RIGHT 1350

#define AUTOPILOT_TURN_SLOW_SPEED_LEFT 1530
#define AUTOPILOT_TURN_SLOW_SPEED_RIGHT 1470

void setup() {
        //Init Compass
        Wire.begin();
        if (!compass.init()) {
	    while (1) ;
        }
        compass.set_orientation(AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD);  // set compass's orientation on aircraft.
        compass.set_offsets(0,0,0);  // set offsets to account for surrounding interference
        compass.set_declination(ToRad(0.0));  // set local difference between magnetic north and true north

        //Fill variables
        wayDegree = 0;
        timer = 0;

        //Uncomment to debug
        Serial.begin(57600);
        Serial.println("hello!");

        delay(3000);
}

void loop() {

     int deltaDegree = getDeltaDegree(getCurrentDegree());
     int turnSpeed = JET_ZERO;
     int dirSpeed = JET_ZERO;
     
     if(abs(deltaDegree)<AUTOPILOT_STOP_OFFSET) {
         dirSpeed = AUTOPILOT_SPEED;         
     }
     
     if(deltaDegree>AUTOPILOT_BACKLASH) {
         if(deltaDegree>AUTOPILOT_STOP_OFFSET) {
           turnSpeed = AUTOPILOT_TURN_HIGH_SPEED_LEFT;         
       } else {
         turnSpeed = AUTOPILOT_TURN_SLOW_SPEED_LEFT;
       }
     }
     if((-deltaDegree)>AUTOPILOT_BACKLASH) {
       if((-deltaDegree)>AUTOPILOT_STOP_OFFSET) {
           turnSpeed = AUTOPILOT_TURN_HIGH_SPEED_RIGHT;
       } else {
           turnSpeed = AUTOPILOT_TURN_SLOW_SPEED_RIGHT;
       }
     }

      Serial.print("Speed:");
      Serial.print(dirSpeed);
      Serial.print(" turn:");
      Serial.print(turnSpeed);
      Serial.print(" delta:");
      Serial.print(deltaDegree);
      Serial.print(" abs:");
      Serial.println(abs(deltaDegree));
     //Выравнивание по координатам
   
  delay(500);
}

int getCurrentDegree() {
  int currentDegree;
  compass.read();
  compass.calculate(0,0);  // roll = 0, pitch = 0 for this example
  currentDegree = ToDeg(compass.heading);
  return currentDegree;
}

int getDeltaDegree(int currentDegree) {
  //currentDegree-wayDegree
  if(currentDegree>0){
     if(wayDegree>(currentDegree-180)) {
       return currentDegree-wayDegree;
     } else {
       return currentDegree-wayDegree-360; 
     }
  } else {
    if(wayDegree<(currentDegree+180)) {
        return currentDegree-wayDegree;       
    } else {
        return currentDegree-wayDegree-360;
    }
  }
}

